#include "terminal_anchor.h"
extern UWB_AnchorInfoTypeDef anchor[6];
extern CircularQueue_TypeDef usart6_queue;
/* UWB TerminalAnchor Task */


uint8_t TERMINAL_SEARCH_DEVICE[] = "{7,aa,bb}";



uint8_t terminal_rcv_buf[64] = {0};
uint16_t terminal_rcv_len = 0;
uint16_t terminal_rcv_expect_len = 0;


void TerminalAnchor_MonitorHost(void) 
{
	uint8_t tmp;
	while(usart6_queue.head != usart6_queue.tail)
	{
		tmp = usart6_queue.buf[usart6_queue.tail++];
		usart6_queue.tail &= QUEUE_BUF_MASK;
		
		if( terminal_rcv_len == 0)
		{
			/*find the '{'*/
			if( tmp == '{')
			{				
				terminal_rcv_buf[terminal_rcv_len++] = tmp;
				terminal_rcv_expect_len = 0;	
			}
		}
		else if( terminal_rcv_expect_len == 0)
		{			
			/*get the length */			
			if(tmp == ',')
			{
				switch(terminal_rcv_len)
				{
					case 2:						
						terminal_rcv_buf[terminal_rcv_len++] = tmp;
						terminal_rcv_expect_len = (terminal_rcv_buf[1] - 0x30) + 2;
						break;
					case 3:					
						terminal_rcv_buf[terminal_rcv_len++] = tmp;
						terminal_rcv_expect_len = (terminal_rcv_buf[2] - 0x30) + ( (terminal_rcv_buf[1] - 0x30) * 10) + 2;
						break;
					case 4:
						terminal_rcv_buf[terminal_rcv_len++] = tmp;
						terminal_rcv_expect_len = (terminal_rcv_buf[3] - 0x30) + ( (terminal_rcv_buf[2] - 0x30) * 10) + ( (terminal_rcv_buf[1] - 0x30) * 100) + 2;					
						break;
					default:
						terminal_rcv_len = 0;
						terminal_rcv_expect_len = 0;
						break;
				}						
			}
			else if(terminal_rcv_len > 3)
			{
				/*exceed the max,error pack*/
				terminal_rcv_len = 0;
				terminal_rcv_expect_len = 0;
			}
			else
			{
				if( tmp < 0x30 || tmp > 0x39)
				{
					/*error data*/
					terminal_rcv_len = 0;
					terminal_rcv_expect_len = 0;
				}
				else
				{
					terminal_rcv_buf[terminal_rcv_len++] = tmp;			
				}
			}			
		}
		else 
		{
			if( tmp == '}')
			{
				/*completed pack*/
				terminal_rcv_buf[terminal_rcv_len++] = tmp;			
				TerminalAnchor_UnpackHost(terminal_rcv_buf,terminal_rcv_len);
				terminal_rcv_len = 0;
				terminal_rcv_expect_len = 0;
			}
			else if(terminal_rcv_len > terminal_rcv_expect_len)
			{
				/*error length*/
				terminal_rcv_len = 0;
				terminal_rcv_expect_len = 0;
			}
			else
			{
				terminal_rcv_buf[terminal_rcv_len++] = tmp;									
			}
		}
	}
}


	uint16_t set_address = 0;
	uint16_t set_xpos = 0,set_ypos = 0;
void TerminalAnchor_UnpackHost(uint8_t *pack,uint16_t length)
{
	uint8_t i;
	switch(length)
	{
		case 9:
			if( String_Compare(pack,TERMINAL_SEARCH_DEVICE,9) == 0 )
			{
				UWB_ScanAnchor(TERMINAL_ANCHOR_ADDRESS);	
			}
			break;
		case 21:
			set_address = (pack[4]-0x30)*(16*16*16) + (pack[5]-0x30)*(16*16) + (pack[6]-0x30)*(16) + (pack[7]-0x30);
			set_xpos = (pack[9]-0x30)*10000 + (pack[10]-0x30)*1000 + (pack[11]-0x30)*100 + (pack[12]-0x30)*10 + (pack[13]-0x30);
			set_ypos = (pack[15]-0x30)*10000 + (pack[16]-0x30)*1000 + (pack[17]-0x30)*100 + (pack[18]-0x30)*10 + (pack[19]-0x30);
		
			for(i=1;i<6;i++)
			{
				if(anchor[i-1].address == set_address)
				{
					UWB_SwitchChannel(i);
					UWB_SendSetAnchorPosition(set_address,set_xpos,set_ypos);    
				}
			}			
			break;
		default:
			break;
	}
}
int res;
uint16_t pack_length = 0;
char tmp[48];
char snd_str[256];
void TerminalAchor_SendDevicePack(uint16_t anchor_cnt,void *anchor_info,uint16_t tag_cnt,void *tag_info)  
{
	Terminal_UnitInfoTypeDef *unit;	
	uint16_t i;	
	pack_length=0;
	res = snprintf(snd_str,255,"{%04d;%d;",pack_length,anchor_cnt);
	if(res != -1)
	{
		pack_length+=res;
	}
	else
		return;
	
	/*fomating the anchor infomation*/
	unit = (Terminal_UnitInfoTypeDef*)anchor_info;
	for(i=0;i<anchor_cnt;i++)
	{
		res = snprintf(tmp,255,"(%d,%d,%d,%d,%d)|",unit->address,unit->channel,unit->x_pos,unit->y_pos,unit->is_led_on);
		if(res != -1)
		{
			strncat(snd_str,tmp,res);
			pack_length+=res;
			
		}
		else
			return;
		if(unit->next!=NULL)
			unit = unit->next;	
		else 
			break;
	}
	/*fomating the anchor infomation*/
	
	
	res = snprintf(tmp,255,";%d;",tag_cnt);
	if(res != -1)
	{
		strncat(snd_str,tmp,res);
		pack_length+=res;
		
	}
	else
		return;	
	
	
	/*fomating the tag infomation*/
	unit = (Terminal_UnitInfoTypeDef*)tag_info;
	for(i=0;i<tag_cnt;i++)
	{
		res = snprintf(tmp,255,"(%d,%d,%d,%d,%d)|",unit->address,unit->channel,unit->x_pos,unit->y_pos,unit->is_led_on);
		if(res != -1)
		{
			strncat(snd_str,tmp,res);
			pack_length+=res;		
		}
		else
			return;
		if(unit->next!=NULL)
			unit = unit->next;	
		else 
			break;

	}		
	/*fomating the tag infomation*/
	
	res = snprintf(tmp,255,"}");
	if(res != -1)
	{
		strncat(snd_str,tmp,1);
		pack_length+=1;
	}
	else
		return;			
	
	res = snprintf(tmp,255,"%04d",pack_length);
	memcpy(snd_str+1,tmp,4);
	
	
	printf("%s",snd_str);
}

